
package com.guazi.trackingtest.deadreckoning.orientation;

import com.guazi.trackingtest.deadreckoning.extra.ExtraFunctions;

//orientation determined using Euler angles and stored inside of Direction Cosine Matrix
//this class was created before the importation of EJML, and therefore does not implement it
public class GyroscopeEulerOrientation {

    private float[][] C;

    //NEU rotation
//    private float[][] rotationNEU= {{0,1,0},
//                                    {1,0,0},
//                                    {0,0,-1}};
//    Android Sensor X --> INS Y
//    Android Sensor Y --> INS X
//    Android Sensor -Z --> INS Z


    public GyroscopeEulerOrientation() {
        C = ExtraFunctions.IDENTITY_MATRIX.clone();
    }

    public GyroscopeEulerOrientation(float[][] initialOrientation) {
        this();
        C = initialOrientation.clone();
    }

    public float[][] getOrientationMatrix(float[] gyroValues) {
//        float wX = gyroValues[0];
//        float wY = gyroValues[1];
//        float wZ = gyroValues[2];

        float wX = gyroValues[1];
        float wY = gyroValues[0];
        float wZ = -gyroValues[2];

        float[][] A = calcMatrixA(wX, wY, wZ);

        calcMatrixC(A);

        return C.clone();
    }

    public float getHeading(float[] gyroValue) {
        getOrientationMatrix(gyroValue);
        return (float) (Math.atan2(C[1][0], C[0][0]));
    }

    private float[][] calcMatrixA(float wX, float wY, float wZ) {

        float[][] A;

        //skew symmetric matrix
        float[][] B = calcMatrixB(wX, wY, wZ);
        float[][] B_sq = ExtraFunctions.multiplyMatrices(B, B);

        float norm = ExtraFunctions.calcNorm(wX, wY, wZ);
        float B_scaleFactor = calcBScaleFactor(norm);
        float B_sq_scaleFactor = calcBSqScaleFactor(norm);

        B = ExtraFunctions.scaleMatrix(B, B_scaleFactor);
        B_sq = ExtraFunctions.scaleMatrix(B_sq, B_sq_scaleFactor);

        A = ExtraFunctions.addMatrices(B, B_sq);
        A = ExtraFunctions.addMatrices(A, ExtraFunctions.IDENTITY_MATRIX);

        return A;
    }

    private float[][] calcMatrixB(float wX, float wY, float wZ) {
//        return (new float[][]{{0, -wZ, wY},
//                              {wZ, 0, -wX},
//                              {-wY, wX, 0}});
          return (new float[][]{{0, wZ, -wY},
                                {-wZ, 0, wX},
                                {wY, -wX, 0}});
    }

    //(sin σ) / σ ≈ 1 - (σ^2 / 3!) + (σ^4 / 5!)
    private float calcBScaleFactor(float sigma) {
        //return (float) ((1 - Math.cos(sigma)) / Math.pow(sigma, 2));
        float sigmaSqOverThreeFactorial = (float) Math.pow(sigma, 2) / ExtraFunctions.factorial(3);
        float sigmaToForthOverFiveFactorial = (float) Math.pow(sigma, 4) / ExtraFunctions.factorial(5);
        return (float) (1.0 - sigmaSqOverThreeFactorial + sigmaToForthOverFiveFactorial);
    }

    //(1 - cos σ) / σ^2 ≈ (1/2) - (σ^2 / 4!) + (σ^4 / 6!)
    private float calcBSqScaleFactor(float sigma) {
        //return (float) (Math.sin(sigma) / sigma);
        float sigmaSqOverFourFactorial = (float) Math.pow(sigma, 2) / ExtraFunctions.factorial(4);
        float sigmaToForthOverSixFactorial = (float) Math.pow(sigma, 4) / ExtraFunctions.factorial(6);
        return (float) (0.5 - sigmaSqOverFourFactorial + sigmaToForthOverSixFactorial);
    }

    //calculate the new DCM depending on the change in orientation
    private void calcMatrixC(float[][] A) {
        C = ExtraFunctions.multiplyMatrices(C, A);
    }

    //rotate DCM to be compatible with the rotation matrix generated by the Magnetic Field data
//    private void rotateMatrixC() {
//
//        float r = (float) Math.atan2(C[2][1], C[2][2]);
//        float p = (float) -Math.asin(C[2][0]);
//        float y = (float) Math.atan2(C[1][0], C[0][0]);
//
//        double[][] C_double = new double[][] {{Math.cos(p)*Math.cos(y), -Math.cos(r)*Math.sin(y) + Math.sin(r)*Math.sin(p)*Math.cos(y), Math.sin(r)*Math.sin(y) + Math.cos(r)*Math.sin(p)*Math.cos(y)},
//                                              {Math.cos(p)*Math.sin(y), Math.cos(r)*Math.cos(y) + Math.sin(r)*Math.sin(p)*Math.sin(y), -Math.sin(r)*Math.cos(y) + Math.cos(r)*Math.sin(p)*Math.sin(y)},
//                                              {-Math.sin(p),            Math.sin(r)*Math.cos(p),                                        Math.cos(r)*Math.cos(p)}};
//
//        //converts double[][] to float[][]
//        for (int row = 0; row < C.length; row++)
//            for (int col = 0; col < C[row].length; col++)
//                C[row][col] = (float)C_double[row][col];
//
//    }

//    public void clearMatrix() {
//        C = ExtraFunctions.IDENTITY_MATRIX;
//    }

}
